home *** CD-ROM | disk | FTP | other *** search
/ Language/OS - Multiplatform Resource Library / LANGUAGE OS.iso / cpp_libs / vector.lha / vector / cmatrix.h < prev    next >
C/C++ Source or Header  |  1991-11-23  |  6KB  |  244 lines
  1. #ifndef _CMATRIX_H
  2. #define _CMATRIX_H
  3.  
  4. #include "cvector.h"
  5. #include "enum.h"
  6.  
  7. // Nest axis enumeration so it must be qualified by e.g. Axis::x
  8. class Axis {
  9. public:
  10.     enum axis { x, y, z };
  11. };
  12.  
  13. #define CartesianMatrixDeclare(T) \
  14.                             \
  15. typedef T CMatType(T)[3][4];                \
  16.                             \
  17. class CMat(T) {                        \
  18. protected:                        \
  19.     T    m[3][4];                    \
  20.                             \
  21. public:                            \
  22.     const T *operator()(int i) const {            \
  23.     return m[i];                    \
  24.     }                            \
  25.     T *operator[](int i) { return m[i]; }        \
  26.     operator CMatType(T)() { return m; }        \
  27.                             \
  28.     CMat(T) &zero();                    \
  29.     CMat(T) &identity();                \
  30. };                            \
  31.                             \
  32. /* V = M * V (column vectors assumed) */        \
  33. CVec(T) operator*(const CMat(T) &m, const CVec(T) &v);    \
  34.                             \
  35. /* M = M * M */                        \
  36. CMat(T) operator*(const CMat(T) &a, const CMat(T) &b);    \
  37.                             \
  38. /* Geometric transformations */                \
  39. CMat(T) rotation_matrix(Enum(Axis,axis) a, T alpha);    \
  40. CMat(T) rotation_matrix(const CVec(T) &axis_, T alpha); \
  41. CMat(T) translation_matrix(const CVec(T) &t);        \
  42. CMat(T) scale_matrix(T s);                \
  43. CMat(T) scale_matrix(const CVec(T) &s);            \
  44.                             \
  45. ostream &operator<<(ostream &o, const CMat(T) &m);
  46.  
  47. #define CartesianMatrixImplement(T) \
  48.                             \
  49. static const int                    \
  50.     X = 0,                        \
  51.     Y = 1,                        \
  52.     Z = 2,                        \
  53.     W = 3;                        \
  54.                             \
  55. CMat(T) &CMat(T)::zero() {                \
  56.     for (int i = X; i <= Z; i++)            \
  57.     for (int j = X; j <= W; j++)            \
  58.         m[i][j] = 0;                \
  59.     return *this;                    \
  60. }                            \
  61.                             \
  62. CMat(T) &CMat(T)::identity() {                \
  63.     for (int i = X; i <= Z; i++)            \
  64.     for (int j = X; j <= W; j++)            \
  65.         m[i][j] = (i == j);                \
  66.     return *this;                    \
  67. }                            \
  68.                             \
  69. CVec(T) operator*(const CMat(T) &m, const CVec(T) &v) { \
  70.     return Vector(                    \
  71.     m(X)[X]*v(X)+m(X)[Y]*v(Y)+m(X)[Z]*v(Z)+m(X)[W], \
  72.     m(Y)[X]*v(X)+m(Y)[Y]*v(Y)+m(Y)[Z]*v(Z)+m(Y)[W], \
  73.     m(Z)[X]*v(X)+m(Z)[Y]*v(Y)+m(Z)[Z]*v(Z)+m(Z)[W]);\
  74. }                            \
  75.                             \
  76. CMat(T) operator*(const CMat(T) &a, const CMat(T) &b) { \
  77.     CMat(T) res;                    \
  78.                             \
  79.     for (int i = X ; i <= Z; i++) {            \
  80.     res[i][X] = a(i)[X] * b(X)[X] +            \
  81.             a(i)[Y] * b(Y)[X] +            \
  82.             a(i)[Z] * b(Z)[X];            \
  83.     res[i][Y] = a(i)[X] * b(X)[Y] +            \
  84.             a(i)[Y] * b(Y)[Y] +            \
  85.             a(i)[Z] * b(Z)[Y];            \
  86.     res[i][Z] = a(i)[X] * b(X)[Z] +            \
  87.             a(i)[Y] * b(Y)[Z] +            \
  88.             a(i)[Z] * b(Z)[Z];            \
  89.     res[i][W] = a(i)[X] * b(X)[W] +            \
  90.             a(i)[Y] * b(Y)[W] +            \
  91.             a(i)[Z] * b(Z)[W] + a(i)[W];    \
  92.     }                            \
  93.                             \
  94.     return res;                        \
  95. }                            \
  96.                             \
  97. static void sincos(T &cval, T &sval, T alpha) {        \
  98.     /* Tolerance before assuming the rotation        \
  99.      *    is aligned with axes                \
  100.      */                            \
  101.     const T tolerance = 1e-10;                \
  102.                             \
  103.     cval = cos(alpha);                    \
  104.     sval = sin(alpha);                    \
  105.                             \
  106.     if (cval > 1 - tolerance) {                \
  107.     cval = 1;                    \
  108.     sval = 0;                    \
  109.     } else if (cval < -1 + tolerance) {            \
  110.     cval = -1;                    \
  111.     sval = 0;                    \
  112.     }                            \
  113.                             \
  114.     if (sval > 1 - tolerance) {                \
  115.     cval = 0;                    \
  116.     sval = 1;                    \
  117.     } else if (sval < -1 + tolerance) {            \
  118.     cval = 0;                    \
  119.     sval = -1;                    \
  120.     }                            \
  121. }                            \
  122.                             \
  123. /* Create a rotation matrix of alpha radians around    \
  124.  *  specified axis (x,y,z).                \
  125.  */                            \
  126. CMat(T) rotation_matrix(Enum(Axis,axis) a, T alpha) {    \
  127.     T ca, sa;                        \
  128.     sincos(ca, sa, alpha);                \
  129.     CMat(T) r;                        \
  130.                             \
  131.     r.zero();                        \
  132.     r[W][W] = 1;                    \
  133. cout << "rotation_matrix: x = "                \
  134.      << Axis::x << ' ' << (int)Axis::x            \
  135.      << " a = " << a << ' ' << (int)a << endl;        \
  136.                             \
  137.     switch (a) {                    \
  138.     case Axis::x:                    \
  139.         r[X][X] = 1;                \
  140.                 r[Y][Y] = ca; r[Y][Z] =-sa; \
  141.                 r[Z][Y] = sa; r[Z][Z] = ca; \
  142.                             \
  143.         break;                    \
  144.     case Axis::y:                    \
  145.         r[X][X] = ca;          r[X][Z] = sa; \
  146.                 r[Y][Y] = 1;        \
  147.         r[Z][X] =-sa;          r[Z][Z] = ca; \
  148.                             \
  149.         break;                    \
  150.     case Axis::z:                    \
  151.         r[X][X] = ca;   r[X][Y] =-sa;        \
  152.         r[Y][X] = sa;   r[Y][Y] = ca;        \
  153.                       r[Z][Z] = 1;    \
  154.                             \
  155.         break;                    \
  156.     default:                    \
  157.         cerr << "rotation_matrix: unknown axis "    \
  158.          << (int)a << ", returning identity"    \
  159.          << endl;                \
  160.         r.identity();                \
  161.         break;                    \
  162.     }                            \
  163.                             \
  164.     return r;                        \
  165. }                            \
  166.                             \
  167. /* Create a rotation matrix around specified vector */    \
  168. CMat(T) rotation_matrix(const CVec(T) &axis_, T alpha) {\
  169.     T ca, sa;                        \
  170.     sincos(ca, sa, alpha);                \
  171.     CMat(T) c, s, r;                    \
  172.                             \
  173.     /* rotation axis must be normalized */        \
  174.     CVec(T) a = axis_;                    \
  175.     a.normalize();                    \
  176.                             \
  177.     /* matrix effecting P' = (1 - cos alpha) (P dot A) A\
  178.      * Rij = (1 - cos alpha) Ai Aj            \
  179.      */                            \
  180.     for (int i = X; i <= Z; i++)            \
  181.     for (int j = X; j <= Z; j++)            \
  182.         c[i][j] = (1 - ca) * a(i) * a(j);        \
  183.                             \
  184.     /* matrix effecting P' = (cos alpha) P + (sin alpha) P ^ A */   \
  185.     s.zero();                                \
  186.     s[X][X] = ca;     s[X][Y] =-sa * a(Z); s[X][Z] = sa * a(Y);  \
  187.     s[Y][X] = sa * a(Z); s[Y][Y] = ca;          s[Y][Z] =-sa * a(X);  \
  188.     s[Z][X] =-sa * a(Y); s[Z][Y] = sa * a(X); s[Z][Z] = ca;        \
  189.                                     \
  190.     /* overall rotation is sum of s and c */        \
  191.     for (i = X; i <= Z; i++)                \
  192.     for (j = X; j <= Z; j++)            \
  193.         r[i][j] = s[i][j] + c[i][j];        \
  194.     r[X][W] = r[Y][W] = r[Z][W] = 0;            \
  195.                             \
  196.     return r;                        \
  197. }                            \
  198.                             \
  199. /* Create a translation matrix by vector t */        \
  200. CMat(T) translation_matrix(const CVec(T) &t) {        \
  201.     CMat(T) r;                         \
  202.                             \
  203.     r.identity();                    \
  204.     r[X][W] = t(X);                    \
  205.     r[Y][W] = t(Y);                    \
  206.     r[Z][W] = t(Z);                    \
  207.                             \
  208.     return r;                        \
  209. }                            \
  210.                             \
  211. /* Create an isotropic scaling matrix */        \
  212. CMat(T) scale_matrix(T s) {                \
  213.     return scale_matrix(CVec(T)(s,s,s));        \
  214. }                            \
  215.                             \
  216. /* Create an anisotropic scaling matrix */        \
  217. CMat(T) scale_matrix(const CVec(T) &s) {        \
  218.     CMat(T) r;                        \
  219.                             \
  220.     r.zero();                        \
  221.     r[X][X] = s(X);                    \
  222.     r[Y][Y] = s(Y);                    \
  223.     r[Z][Z] = s(Z);                    \
  224.                             \
  225.     return r;                        \
  226. }                            \
  227.                             \
  228. ostream &operator<<(ostream &s, const CMat(T) &m) {    \
  229.     for (int i = X; i <= Z; i++)            \
  230.     s << "\t" << "( "                \
  231.       << m(i)[X] << " "                \
  232.       << m(i)[Y] << " "                \
  233.       << m(i)[Z] << " "                \
  234.       << m(i)[W] << " )\n";                \
  235.                             \
  236.     return s;                        \
  237. }
  238.  
  239. CartesianMatrixDeclare(float);
  240.  
  241. typedef CMat(float) Matrix;
  242.  
  243. #endif /*_CMATRIX_H*/
  244.